// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: t -*-

/*
jeti.pde

Author:     Uwe Gartmann

This firmware is free software; you can redistribute it and/or
modify it under the terms of the GNU Lesser General Public
License as published by the Free Software Foundation; either
version 2.1 of the License, or (at your option) any later version.
*/

// menu structure
//
// [status] - [data1] - [data2] - [data3] - [data4] - [dataXY]
//     |_________|  _______|_________|_________|_________|
//     |   
// [SETUP1] - [SETUP2] - [SETUPXY]
//     |         |          |
// [PARAM1.1] [PARAM2.1] [PARAMX.Y]
//     |         |          |
// [PARAM1.2] [PARAM2.2] [PARAMX.Y]
//     |         |          |
// [PARAM2.3] [PARAM2.3] [PARAMX.Y]

/// Definitions
#define LCDMaxPos 32
#define LAST_DATA_MENU 20
#define LAST_SETUP_MENU 400
#define MAX_STATUS_DELAY 10
#define ArrowLeft (char) 247
#define ArrowRight (char) 246
#define ArrowUp (char) 197
#define ArrowDown (char) 245

/// Includes

/// Declarations
// Statusmenu
const uint16_t mnuSTATUS	= 0;
const uint16_t mnuDATA1 	= 1;
const uint16_t mnuDATA2 	= 2;
const uint16_t mnuDATA3 	= 3;
const uint16_t mnuDATA4 	= 4;
const uint16_t mnuDATA5		= 5;
const uint16_t mnuDATA6		= 6;
const uint16_t mnuDATA7		= 7;
const uint16_t mnuDATA8		= 8;
const uint16_t mnuDATA9		= 9;
const uint16_t mnuDATA10	= 10;
const uint16_t mnuDATA11	= 11;
const uint16_t mnuDATA12	= 12;
const uint16_t mnuDATA13	= 13;
const uint16_t mnuDATA14	= 14;
const uint16_t mnuDATA15	= 15;
const uint16_t mnuDATA16	= 16;
const uint16_t mnuDATA17	= 17;
const uint16_t mnuDATA18	= 18;
const uint16_t mnuDATA19	= 19;
const uint16_t mnuDATA20	= 20;
const uint16_t mnuMESSAGE	= 99;

// Setup 1
const uint16_t mnuSETUP1 	= 100;
const uint16_t mnuPARAM11	= 101;
const uint16_t mnuPARAM12	= 102;
const uint16_t mnuPARAM13	= 103;
const uint16_t mnuPARAM14	= 104;

// Setup 2
const uint16_t mnuSETUP2 	= 200;
const uint16_t mnuPARAM21	= 201;
const uint16_t mnuPARAM22	= 202;
const uint16_t mnuPARAM23	= 203;
const uint16_t mnuPARAM24	= 204;

// Setup 3
const uint16_t mnuSETUP3 	= 300;
const uint16_t mnuPARAM31	= 301;
const uint16_t mnuPARAM32	= 302;
const uint16_t mnuPARAM33	= 303;
const uint16_t mnuPARAM34	= 304;

// Setup 4
const uint16_t mnuSETUP4 	= 400;
const uint16_t mnuPARAM41	= 401;
const uint16_t mnuPARAM42	= 402;
const uint16_t mnuPARAM43	= 403;
const uint16_t mnuPARAM44	= 404;

extern uint8_t reverse_roll;		//radio.pde
extern uint8_t reverse_pitch;		//radio.pde
extern uint8_t reverse_rudder;		//radio.pde
extern uint8_t delta_mix;			//radio.pde
extern char* control_mode_names[];	//radio.pde

static uint16_t MnuPtr = mnuSTATUS;
static uint16_t SaveMnuPtr = mnuSTATUS;
static uint8_t StatusDelay = 0;
static uint8_t statustxt[16];
static uint8_t dummy = 160;

void initJeti() {
	// Init Jeti Serial Port
	Jeti.begin();
	//        12345678901234567890123456789012	
	Jeti.print(" *** X-DIY ***   APM/IMU V0.2b");
}

void messageJeti(const char *str)
{
	byte length = strlen(str);
	for (byte i = 0; i<16; i++) {
		if (i<length) {
			statustxt[i] = str[i];
		} else {
			statustxt[i] = 32; // if text is less 16 chars -> fill up with SPACE
		}
	}
	SaveMnuPtr = MnuPtr;
	MnuPtr = mnuMESSAGE;
}

void updateJeti() {

	uint8_t Buttons = Jeti.readbuttons();	
	jeti_menuctrl(Buttons);
	//IO.DebugPin(DEBUG_PINB, ON);
	switch (MnuPtr) {
		case mnuSTATUS: 
			Jeti.clear(0);
			Jeti.print("Mode> ");
			Jeti.print(control_mode_names[getControlPos()]);
			Jeti.setcursor(LCDLine2);
			Jeti.print("Settings");
			Jeti.print(ArrowDown);
			Jeti.print("  Mntr");
			Jeti.print(ArrowRight);
			break;
			
		case mnuMESSAGE:
			switch (StatusDelay) {
				case 0:
					Jeti.clear(0);
					StatusDelay++;
					for (byte i = 0; i<16; i++) {
						Jeti.print(statustxt[i]);
					}
					break;
				case MAX_STATUS_DELAY:
					MnuPtr = SaveMnuPtr;
					StatusDelay = 0;
					break;
				default:
					StatusDelay++;
			}
			break;
			
		case mnuDATA1:
			Jeti.clear(0);
			Jeti.print("GPS> ");
			if (GPS.fix == 1) {
				Jeti.print("FIX");
				Jeti.setcursor(LCDLine2);
				Jeti.print("Alt: ");
				Jeti.print((int)GPS.altitude/100);
				Jeti.print(" SAT:");
				Jeti.print((int)GPS.num_sats);
			} else {
				Jeti.print("no FIX");
				Jeti.setcursor(LCDLine2);
				Jeti.print("ToW: ");
				Jeti.print((long)GPS.time);
			}
			break;

		case mnuDATA2:
			Jeti.clear(0);
			Jeti.print("Lat ");
			Jeti.print((float)GPS.latitude/10000000, 9);
			Jeti.setcursor(LCDLine2);
			Jeti.print("Lon  ");
			Jeti.print((float)GPS.longitude/10000000, 9);
			break;
			
		case mnuDATA3:
			Jeti.clear(0);
			updateReverseSwitches();
			Jeti.print("REVERSE>");
			Jeti.print((delta_mix == 1) ? " Delta" : " Plane");
			Jeti.setcursor(LCDLine2);
			Jeti.print((reverse_roll == 1) ? "+" : "-");
			Jeti.print("Rol ");
			Jeti.print((reverse_pitch == 1) ? "+" : "-");
			Jeti.print("Pit ");
			Jeti.print((reverse_rudder == 1) ? "+" : "-");
			Jeti.print("Rud");
			break;

		case mnuDATA4:
			Jeti.clear(0);
			Jeti.print("freeRAM> ");
			Jeti.print((long) freeRAM());
			break;
			
		case mnuDATA5:
			Jeti.clear(0);
			Jeti.print("ACC> X:Y:Z");
			Jeti.setcursor(LCDLine2);
			Jeti.print((float)IMU.Sensor[IMU_accX],0);
			Jeti.print(":");
			Jeti.print((float)IMU.Sensor[IMU_accY],0);
			Jeti.print(":");
			Jeti.print((float)IMU.Sensor[IMU_accZ],0);
			break;
		
		case mnuDATA6:
			Jeti.clear(0);
			Jeti.print("GYRO> R:P:Y");
			Jeti.setcursor(LCDLine2);
			Jeti.print((float)IMU.Sensor[IMU_roll],0);
			Jeti.print(":");
			Jeti.print((float)IMU.Sensor[IMU_pitch],0);
			Jeti.print(":");
			Jeti.print((float)IMU.Sensor[IMU_yaw],0);
			break;
			
		case mnuDATA7:
			Jeti.clear(0);
			Jeti.print("COMP> Heading");
			Jeti.setcursor(LCDLine2);
			Jeti.print((int)ToDeg(Compass.Heading));
			break;

		case mnuDATA8:
			Jeti.clear(0);
			Jeti.print("O>");
			Jeti.print((int) IMU.Offset[IMU_roll]);
			Jeti.print(",");
			Jeti.print((int) IMU.Offset[IMU_pitch]);
			Jeti.print(",");
			Jeti.print((int) IMU.Offset[IMU_yaw]);
			Jeti.setcursor(LCDLine2);
			Jeti.print((int) IMU.Offset[IMU_accX]);
			Jeti.print(",");
			Jeti.print((int) IMU.Offset[IMU_accY]);
			Jeti.print(",");
			Jeti.print((int) IMU.Offset[IMU_accZ]);
			break;

			case mnuDATA9:
			Jeti.clear(0);
			Jeti.print("COMPASS> X:Y:Z");
			Jeti.setcursor(LCDLine2);
			Jeti.print((int) Compass.Mag_X);
			Jeti.print(",");
			Jeti.print((int) Compass.Mag_Y);
			Jeti.print(",");
			Jeti.print((int) Compass.Mag_Z);
			break;
			
		case mnuDATA10:
			Jeti.clear(0);
			Jeti.print("#1:");
			Jeti.print((int)RC.CH_In[0]);
			Jeti.print(" #2:");
			Jeti.print((int)RC.CH_In[1]);
			Jeti.setcursor(LCDLine2);
			Jeti.print("#3:");
			Jeti.print((int)RC.CH_In[2]);
			Jeti.print(" #4:");
			Jeti.print((int)RC.CH_In[3]);
			break;

		case mnuDATA11:
			Jeti.clear(0);
			Jeti.print("#Q:");
			Jeti.print((int)RC.CH_Out[0]);
			Jeti.print(" #H:");
			Jeti.print((int)RC.CH_Out[1]);
			Jeti.setcursor(LCDLine2);
			Jeti.print("#G:");
			Jeti.print((int)RC.CH_Out[2]);
			Jeti.print(" #S:");
			Jeti.print((int)RC.CH_Out[3]);
			break;
			
		case mnuDATA12:
			Jeti.clear(0);
			Jeti.print("AIRSPEED> Raw");
			Jeti.setcursor(LCDLine2);
			Jeti.print((int)IMU.Sensor[IMU_dps]);
			break;
			
		case mnuDATA13:
			Jeti.clear(0);
			Jeti.print("IMU-TEMP> Raw");
			Jeti.setcursor(LCDLine2);
			Jeti.print((int)IMU.Sensor[IMU_temp]);
			break;
		
		case mnuDATA14:
			Jeti.clear(0);
			Jeti.print("ALTI> Pressure");
			Jeti.setcursor(LCDLine2);
			Jeti.print((long)Altimeter.Press/100);
			Jeti.print(" hPa");
			break;		
		
		case mnuDATA15:
			Jeti.clear(0);
			Jeti.print("ALTI> Temp");
			Jeti.setcursor(LCDLine2);
			Jeti.print((long)Altimeter.Temp/10);
			Jeti.print(" Grad");
			break;	
			
		case mnuSETUP1:
			Jeti.clear(0);
			Jeti.print("Setups         ");
			Jeti.print(ArrowRight);
			Jeti.setcursor(LCDLine2);
			Jeti.print("Compass ");
			Jeti.print(ArrowDown);
			break;
		
		case mnuPARAM11:
			Jeti.clear(0);
			Jeti.print("Compass Decl.");
			Jeti.setcursor(LCDLine2);
			Jeti.print(ArrowLeft);
			Jeti.setcursor(20);
			if (changeValue(&Compass.declination, 0.1, Buttons))
				saveCompassDeclination();
			if (Compass.declination >= 0) {
				Jeti.print("East: ");
			} else {
				Jeti.print("West: ");
			}
			Jeti.print((float) abs(Compass.declination),1);
			Jeti.setcursor(32);
			Jeti.print(ArrowRight);
			break;

		case mnuPARAM12:
			Jeti.clear(0);
			Jeti.print(ArrowLeft);
			Jeti.print("E ");
			Jeti.print((int) Compass.Mag_Xmax);
			Jeti.print(",");
			Jeti.print((int) Compass.Mag_Ymax);
			Jeti.print(",");
			Jeti.print((int) Compass.Mag_Zmax);
			Jeti.setcursor(14);
			Jeti.print(" S");
			Jeti.print(ArrowRight);
			Jeti.setcursor(LCDLine2);
			Jeti.print((int) Compass.Mag_Xmin);
			Jeti.print(",");
			Jeti.print((int) Compass.Mag_Ymin);
			Jeti.print(",");
			Jeti.print((int) Compass.Mag_Zmin);			
			switch (Buttons) {
				case Jeti_key_left:
					clearCompassMaxMinValue();
					messageJeti("Data reseted");
					break;
				case Jeti_key_right:
					saveCompassMaxMinValues();
					messageJeti("Data saved");
			}
			break;

		case mnuSETUP2:
			Jeti.clear(0);
			Jeti.print("Setups          ");
			Jeti.print(ArrowRight);
			Jeti.setcursor(LCDLine2);
			Jeti.print("ACC/GYRO ");
			Jeti.print(ArrowDown);
			break;

		case mnuPARAM21:
			Jeti.clear(0);
			Jeti.print(ArrowLeft);
			Jeti.print("E ");
			Jeti.print((int) IMU.Sensor[IMU_roll]);
			Jeti.print(",");
			Jeti.print((int) IMU.Sensor[IMU_pitch]);
			Jeti.print(",");
			Jeti.print((int) IMU.Sensor[IMU_yaw]);
			Jeti.setcursor(14);
			Jeti.print(" S");
			Jeti.print(ArrowRight);
			Jeti.setcursor(LCDLine2);
			Jeti.print((int) IMU.Sensor[IMU_accX]);
			Jeti.print(",");
			Jeti.print((int) IMU.Sensor[IMU_accY]);
			Jeti.print(",");
			Jeti.print((int) IMU.Sensor[IMU_accZ]);			
			switch (Buttons) {
				case Jeti_key_right:
					saveIMUOffsets();
					messageJeti("Offsets saved");
			}
			break;
			
		default:
			Jeti.clear(0);
			Jeti.print("Menu: #");
			Jeti.print((int) MnuPtr);
			Jeti.print(" n/a");
			break;
	}
	//IO.DebugPin(DEBUG_PINB, OFF);
}

void jeti_menuctrl(uint8_t btns) {
	// Algorithm for menubuttons
	// if MnuPtr == 0			=> Status
	// if MnuPtr < 100			=> Data
	// if (MnuPtr mod 100) = 0	=> Setup
	// if (MnuPtr mod 100) <> 0	=> Setup parameter
	
	if (MnuPtr == mnuSTATUS) {				// MnuPtr = Status
		switch (btns) {
			case Jeti_key_right:
				MnuPtr += 1;
				break;

			case Jeti_key_left:
				MnuPtr = LAST_DATA_MENU;
				break;

			case Jeti_key_down:
				MnuPtr = mnuSETUP1;
				break;
		}
	}
	else if (MnuPtr < 100) {		// MnuPtr = Data
		switch (btns) {
			case Jeti_key_right:
				if (MnuPtr < LAST_DATA_MENU) MnuPtr += 1; else MnuPtr = mnuSTATUS;
				break;

			case Jeti_key_left:
				if (MnuPtr > mnuSTATUS) MnuPtr -= 1; else MnuPtr = LAST_DATA_MENU;
				break;
		}
	}
	else if (MnuPtr % 100 == 0) {	// MnuPtr = Setup
		switch (btns) {
			case Jeti_key_right:
				if (MnuPtr < LAST_SETUP_MENU) MnuPtr += 100; else MnuPtr = mnuSETUP1;
				break;

			case Jeti_key_left:
				if (MnuPtr > mnuSETUP1) MnuPtr -= 100; else MnuPtr = LAST_SETUP_MENU;
				break;
				
			case Jeti_key_up:
				MnuPtr = mnuSTATUS;
				break;

			case Jeti_key_down:
				MnuPtr += 1;
				break;
		}				
	}
	else {							// MnuPtr = Parameter
		switch (btns) {
			case Jeti_key_down:
				if ((MnuPtr % 100) < 4) MnuPtr += 1;
				break;

			case Jeti_key_up:
				if ((MnuPtr % 100) > 0) MnuPtr -= 1;
				break;
		}		
	}
}

bool changeValue(float * v, float s, uint8_t btns) {	// v = value	s = step (0.1, 0.5, 1.0)

	switch (btns) {
		case Jeti_key_left:
			*v -= s;
			return true;
			//break;
		case Jeti_key_right:
			*v += s;
			return true;
			//break;
	}		
}

